#include <dalelib/maths/xform.h>

namespace dalelib
{

const xform xform::identity(m3::identity,v3::zero);


void xform::print(const std::string _Str) const
{
	if( _Str!="" ) printf( "%s\n", _Str.c_str() );
	printf( "%.3f %.3f %.3f    %.3f\n",   _rot[0], _rot[3], _rot[6], _pos[0] );
	printf( "%.3f %.3f %.3f    %.3f\n",   _rot[1], _rot[4], _rot[7], _pos[1] );
	printf( "%.3f %.3f %.3f    %.3f\n\n", _rot[2], _rot[5], _rot[8], _pos[2] );
}

xform xform::operator*( const xform& _Value ) const
{
	const float* r = _Value.rot().ptr();
	const float* p = _Value.pos().ptr();
	return xform(
		m3(
			r[0]*_rot[0] + r[1]*_rot[3] + r[2]*_rot[6],
			r[0]*_rot[1] + r[1]*_rot[4] + r[2]*_rot[7],
			r[0]*_rot[2] + r[1]*_rot[5] + r[2]*_rot[8],

			r[3]*_rot[0] + r[4]*_rot[3] + r[5]*_rot[6],
			r[3]*_rot[1] + r[4]*_rot[4] + r[5]*_rot[7],
			r[3]*_rot[2] + r[4]*_rot[5] + r[5]*_rot[8],

			r[6]*_rot[0] + r[7]*_rot[3] + r[8]*_rot[6],
			r[6]*_rot[1] + r[7]*_rot[4] + r[8]*_rot[7],
			r[6]*_rot[2] + r[7]*_rot[5] + r[8]*_rot[8]),
		v3(
			p[0]*_rot[0] + p[1]*_rot[3] + p[2]*_rot[6],
			p[0]*_rot[1] + p[1]*_rot[4] + p[2]*_rot[7],
			p[0]*_rot[2] + p[1]*_rot[5] + p[2]*_rot[8])
		);
}

} // namespace dalelib
